;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Images
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defclass image ()
  ((rows :initarg :rows :reader rows)
   (cols :initarg :cols :reader cols)
   (depth :initarg :depth :reader depth)
   (pixels :initarg :pixels :accessor pixels))
  (:documentation "Class of images."))

;;Method: get-pixel
(defgeneric get-pixel (img sh)
  (:documentation "Returns the value of pixel (i,j) if the given image"))

(defmethod get-pixel ((img image) sh)
  (nth-value 0 (gethash sh (pixels img))))

;;Method: set-pixel
(defgeneric set-pixel (img sh value)
  (:documentation "Set pixel (i,j) of img to the given value"))

(defmethod set-pixel ((img image) sh (value number))
  (setf (gethash sh (pixels img)) value))


;;Function: create-image-from-file
(defun create-image-from-file (filename)
  (with-open-file (in filename)
    (with-standard-io-syntax
      (progn
	(read-line in) ; P2
	(read-line in) ; comment #
	(let* ((n (read in))
	       (m (read in))
	       (depth (read in))
	       (data (make-hash-table :test #'equalp)))
	  (progn
	    (loop for i from 1 to m do
		 (loop for j from 1 to n do
		      (setf (gethash (list i j) data) (read in))))
	    (make-instance 'image :rows m :cols n :depth depth :pixels data)))))))



(defgeneric locate-color (img color-value)
  (:documentation "Return a list of pixel coordinates that match a given color."))

(defmethod locate-color ((img image) (color-value integer))
  (let ((data (loop for key being the hash-keys of (pixels img) collecting key)))
    (remove-if-not #'(lambda (x) (equalp (gethash x (pixels img)) color-value)) data)))



(defgeneric images-have-same-dimension (img1 img2)
  (:documentation "Check if the dimensions of img1 and img2 are the same"))

(defmethod images-have-same-dimension ((img1 image) (img2 image))
  (and (= (rows img1) (rows img2)) (= (cols img1) (cols img2))))



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Shifts
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun shift (i j)
  (list i j))

(defun shift-sum (sh1 sh2)
    (shift (+ (first sh1) (first sh2))
		  (+ (second sh1) (second sh2))))

(defun shift-difference (sh1 sh2)
    (shift (- (first sh1) (first sh2))
		  (- (second sh1) (second sh2))))

(defun shift-abs (sh)
  (let ((i (abs (first sh)))
	(j (abs (second sh))))
    (shift (max i j) (min i j))))


(defun top-exists (shift)
  (> (car shift) 1))

(defun top (shift)
  (shift (- (car shift) 1) (car (cdr shift))))

(defun left-exists (shift)
  (> (car (cdr shift)) 1))

(defun left (shift)
  (shift (car shift) (- (car (cdr shift)) 1)))

(defparameter *matrice-dei-pesi* (make-hash-table :test #'equalp))


(defun g-term (sh)
  (let ((tmp (shift-abs sh)))
    (or (gethash tmp *matrice-dei-pesi*)
	(add-g-term tmp))))

(defun add-g-term (sh)
  (setf (gethash sh *matrice-dei-pesi*) (g sh)))


(defun g (sh)
  (let ((i (first sh))
	(j (second sh)))
    (sqrt (+ (* i i) (* j j)))))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Ants
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun ant-path ()
  (make-hash-table :test #'equalp))

(defun get-ant-path-quark (shift path)
  (gethash shift path))

(defun set-ant-path-quark (shift path quark)
  (setf (gethash shift path) quark))


(defun path-energy (path)
  (loop for key being the hash-keys of path using (hash-value value) summing (quark-value value)))




;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Pheromone trials
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun empty-pheromone-trial ()
  (make-hash-table :test #'equalp))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Quarks e liste di quarks
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


;; Questa funzione restituisce una lista di spostamenti possibili,
;; ciascuno coredato dal proprio feromOne
;; gli elementi della lista sono detti QUARK
(defun build-base-elements (hash others)
  (let ((quark-list (loop for key being the hash-keys of hash using (hash-value value) collecting (quark key value))))
    (loop for sh in others do (or (gethash sh hash) (push (quark sh 1) quark-list)))
    quark-list))


(defun update-quark-list (quark-list img1 img2 base-pixel path alpha beta)
  (loop for quark in quark-list collecting (update-quark quark img1 img2 base-pixel path alpha beta)))

(defun update-quark (quark img1 img2 base-pixel path alpha beta)
  (let* ((shift (quark-shift quark))
	 (ph-value (quark-value quark))
	 (energy-value (energy img1 img2 base-pixel shift path)))
    (quark  shift (* (expt ph-value alpha) (expt (eta energy-value 100000 1) beta)))))



;;Scelta di un elemento in una lista di quark
(defun choose-quark (quark-list)
  (let* ((S (float (loop for quark in quark-list summing (quark-value quark))))
	 (r (if (> S 0) (random S) 0))
	 (sub-quark (remove-if #'(lambda (quark) (< r (quark-value quark))) quark-list))
	 (n (random (length sub-quark))))
    (nth n sub-quark)))


;; Un quark è fatto da una lista il cui primo elemento è una lista di due elementi
;; mentre il secondo è Un valore ((i j) p)
(defun quark (sh value)
  (list :shift sh :value (float value)))

;; Alcune funzioni sui quark

;ritorna la coppia (i j) rappresentante lo spostamento
(defun quark-shift (quark)
  (getf quark :shift))
;ritorna il valore p
(defun quark-value (quark)
  (getf quark :value))


;setta il value del quark
(defun set-quark-value (quark value)
  (setf  (getf quark :value) value))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Funzioni di supporto
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;
;;
;;
;;
;;Euristica:	    y ^
;;		      |
;;		    B +--
;;		      |	 \----
;;		      |	      \----
;;		      |		   \-----
;;		      |			 \----
;;		      |			      \----
;;    ----------------+----------------------------\------------>
;;		      |                            A            x
;;		      |
;;		      |
;;		      |
;;		      |
;;		      |
;;		      |
;; y = -(B/A)x+B
(defun eta (x A B)
  (cond
    ((>= x A) 0)
    (T (+ B (* x (- (/ B A)))))))


;; Quadrato
(defun square (x)
  (* x x))

(defun energy-fidelity (img1 img2 base-pixel shift)
   (square (- (get-pixel img1 base-pixel) (get-pixel img2 (shift-sum base-pixel shift)))))

(defun energy-top (shift path)
  (g-term (shift-difference shift (quark-shift (get-ant-path-quark (top shift) path)))))

(defun energy-left (shift path)
  (g-term (shift-difference shift (quark-shift (get-ant-path-quark (left shift) path)))))

(defun energy (img1 img2 base-pixel shift path)
  (+
   (energy-fidelity img1 img2 base-pixel shift)
   (if (top-exists base-pixel)
       (energy-top base-pixel path)
       0)
   (if (left-exists base-pixel)
       (energy-left base-pixel path)
       0)))





;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; ACO
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


(defun aco (img1 img2 ant-paths pheromone max-iteration)
  (loop for iteration from 1 to max-iteration do
       (progn 
	 (format t "Iterazione ~A~%" iteration)
	 (loop for ant-path in ant-paths do 
	      (loop for m from 1 to (rows img1) do
		   (loop for n from 1 to (cols img1) do
			(set-ant-path-quark (shift m n) ant-path 
			 (choose-quark 
			  (update-quark-list 
			   (build-base-elements (gethash (shift m n) pheromone)
						(build-other-choices (shift m n) img1 img2 ant-path)) 
			   img1 img2 (shift m n) ant-path 3 4)))))))))

(defun aco2 (img1 img2 ant-paths pheromone max-iteration)
  ;;Iterazione sul numero di formiche
  (loop for ant-path in ant-paths do
       (progn
	 (format t "Inizializzazione formica!~%")
	 ;;Iterazione sul numero massimo di iterazioni
	 (loop for iteration from 1 to max-iteration do
	      (progn
		(format t "Iterazione ~A~%" iteration)
		(loop for m from 1 to (rows img1) do
		     (loop for n from 1 to (cols img1) do
			  (let* ((base-pixel (shift m n))
				 (other-choices (build-other-choices base-pixel img1 img2 ant-path))
				 (pheromone-trial (gethash base-pixel pheromone))
				 (base-elements (build-base-elements pheromone-trial other-choices))
				 (quarks-updated (update-quark-list base-elements img1 img2 base-pixel ant-path 3 4))
				 (choosen-quark (choose-quark quarks-updated)))
			    (format t "Elementi di base: ~{~A~}~%" base-elements)
			    (format t "Elementi aggiornati: ~{~A~}~%" quarks-updated)
			    (format t "Ho scelto il quark ~A~%" choosen-quark)
			    (set-ant-path-quark base-pixel ant-path choosen-quark)))))))))


				 


(defun build-other-choices (sh img1 img2 path)
  (list (shift 2 3) (shift 3 4) (shift 3 5)))



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; User interface
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Alcuni esempi di test
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defparameter *pheromone* (make-hash-table :test #'equalp))

(defparameter *test* (create-image-from-file "test.pgm"))
(defparameter *test-shifted* (create-image-from-file "test-shifted.pgm"))

(defparameter *test-small* (create-image-from-file "test.pgm"))
(defparameter *test-shifted-small* (create-image-from-file "test-shifted.pgm"))

(loop for m from 1 to (rows *test*) do
     (loop for n from 1 to (cols *test*) do
	  (setf (gethash (shift m n) *pheromone*) (empty-pheromone-trial))))

(defparameter *prova* (build-base-elements (gethash '(1 1) *pheromone*) '((1 2) (2 3))))



(defparameter *ants* (loop for i from 1 to 3 collecting (ant-path)))




(defconstant +max+ 10)


;(defun reset ()
 ; (progn
;    (setq *ants* (loop for i from 1 to 3 collecting (ant-path)))
;    (setq *pheromone* (make-hash-table :test #'equalp))
;    (loop for m from 1 to (rows *test*) do
;     (loop for n from 1 to (cols *test*) do
;	  (setf (gethash (shift m n) *pheromone*) (empty-pheromone-trial))))))

